##########################
quadPts<- c(-0.973906528517172,
         -0.865063366688985,
         -0.679409568299024,
         -0.433395394129247,
         -0.148874338981631,
          0.148874338981631,
          0.433395394129247,
          0.679409568299024,
          0.865063366688985,
          0.973906528517172);
quadWgts<- c(0.066671344308688,
        0.149451349150581,
        0.219086362515982,
        0.269266719309996,
        0.295524224714753,
        0.295524224714753,
        0.269266719309996,
        0.219086362515982,
        0.149451349150581,
        0.066671344308688);


library(lattice)
library(SparseM)

tol<- 1E-18
cellwidth<- 1
centroid <- as.vector(outer(seq(from=.5, by=1, length=9), seq(from=.5, by=1, length=9), function(x, y) complex(real=x, imaginary=y)))
quadWgts.mat<- as.matrix.csr(diag(1/4, length(centroid)) %x% as.vector(outer(quadWgts, quadWgts, "*")))
evalpt   <- as.vector(outer(as.vector(outer(quadPts/2, quadPts/2, function(x, y) complex(real=x, imaginary=y))), centroid, "+"))

incident.mat<- matrix(as.integer(0), nrow=8, ncol=81)
incident.mat[1, 10:18]<- incident.mat[2,28:36]<- incident.mat[3,45:54]<-
incident.mat[4,64:72]<- as.integer(1)
incident.mat[5, seq(from=2, by=9, length=9)]<-
incident.mat[6, seq(from=4, by=9, length=9)]<-
incident.mat[7, seq(from=6, by=9, length=9)]<-
incident.mat[8, seq(from=8, by=9, length=9)]<- as.integer(1)

observed<- matrix(1, nrow=1, ncol=8)

#gaussian.intKernel<- function(centroid, y, bw, cellwidth) {
#    (pnorm(Re(centroid)+cellwidth/2, mean=Re(y), sd=bw[1]) - pnorm(Re(centroid)-cellwidth/2, mean=Re(y), sd=bw[1]))*
#    (pnorm(Im(centroid)+cellwidth/2, mean=Im(y), sd=bw[2]) - pnorm(Im(centroid)-cellwidth/2, mean=Im(y), sd=bw[2]))
#}

#gaussian.kernel<- function(x, y, bw, rho) exp(-.5*((x/bw[1])^2+(y/bw[2])^2 - 2*rho*x*y/prod(bw))/(1-rho^2))/(2*pi*prod(bw)*sqrt(1-rho^2))
gaussian.kernel<- function(x, y, bw, theta) {
  u<- x-y
  #u1<- Re(u)
  #u2<- Im(u)
  #exp(-.5*((u1/bw[1])^2+(u2/bw[2])^2 - 2*rho*u1*u2/prod(bw))/(1-rho^2))/(2*pi*sqrt((1+rho)*(1-rho))*prod(bw))
  u<- matrix(c(cos(theta), -sin(theta), sin(theta), cos(theta)), nrow=2, ncol=2) %*% t(matrix(c(Re(u), Im(u)), ncol=2))
  return(dnorm(x=u[1,]/bw[1])*dnorm(x=u[2,]/bw[2])/prod(bw))
}

#gk4<- function(x, bw, rho) exp(-.5*(((x[1]-x[3])/bw[1])^2+((x[2]-x[4])/bw[2])^2 - 2*rho*(x[1]-x[3])*(x[2]-x[4])/prod(bw))/(1-rho^2))/(2*pi*prod(bw)*sqrt(1-rho^2))
#gk2<- function(x, y, bw, rho) exp(-.5*(((x[1]-y[1])/bw[1])^2+((x[2]-y[2])/bw[2])^2 - 2*rho*(x[1]-y[1])*(x[2]-y[2])/prod(bw))/(1-rho^2))/(2*pi*prod(bw)*sqrt(1-rho^2))






#### EM ###################
#intKernel.mat<- outer(evalpt, evalpt, gaussian.kernel, bw=rep(0, 2), theta=0)
#K.mat<- t(quadWgts.mat) %*% intKernel.mat %*% quadWgts.mat
#as.matrix(K.mat)
#rep(1, 81) %*% K.mat
K.mat<- as(rep(1, 81), "matrix.diag.csr")
##### initial value #####
Lambda.old<- matrix(1, nrow=1, ncol=length(centroid));
niter<- as.integer(1)


Lambda.old<- matrix(0, nrow=1, ncol=length(centroid));
Lambda.old[1, c(11, 31, 51, 71)]<- 1
#Lambda.old<- matrix(0, nrow=1, ncol=length(centroid));
#Lambda.old[1, c(17, 33, 49, 65)]<- 1
#### EMS iteration ####
repeat {
    ##### EM step #####
    V.inv<- diag(1/as.vector(Lambda.old %*% t(incident.mat)))
    tmpEM<- (observed %*% V.inv %*% incident.mat) %*% diag(as.vector(Lambda.old)) %*% diag(1/8, 81)
    ##### S step #####
    Lambda<-  tmpEM %*% K.mat

    if (sum( (as.vector(as.matrix(Lambda)) - as.vector(as.matrix(Lambda.old)))^2) > tol) {
        Lambda.old<- as.vector(as.matrix(Lambda));
        niter<- as.integer(1+niter);
    }
    else {
        #lambda<- tmpEM %*% K_x.mat;
        Lambda<- as.vector(as.matrix(Lambda))
        break;
    }
}
Lambda.3<- Lambda
K.mat.em<- K.mat

#dev.off()


####### theta = 0 ############
##### EMS #################################
bandwidth<- c(.35, .35)
intKernel.mat<- outer(evalpt, evalpt, gaussian.kernel, bw=bandwidth, theta=0)
K.mat<- t(quadWgts.mat) %*% intKernel.mat %*% quadWgts.mat
#as.matrix(K.mat)
niter<- as.integer(1)

##### initial value #####
Lambda.old<- matrix(1, nrow=1, ncol=length(centroid));
#Lambda.old[1, c(17, 33, 49, 65)]<- .25
#### EMS iteration ####
repeat {
    ##### EM step #####
    V.inv<- diag(1/as.vector(Lambda.old %*% t(incident.mat)))
    tmpEM<- (observed %*% V.inv %*% incident.mat) %*% diag(as.vector(Lambda.old)) %*% diag(1/8, 81)
    ##### S step #####
    Lambda<-  tmpEM %*% K.mat

    if (sum( (as.vector(as.matrix(Lambda)) - as.vector(as.matrix(Lambda.old)))^2) > tol) {
        Lambda.old<- as.vector(as.matrix(Lambda));
        niter<- as.integer(1+niter);
    }
    else {
        #lambda<- tmpEM %*% K_x.mat;
        Lambda<- as.vector(as.matrix(Lambda))
        break;
    }
}
Lambda.0<- Lambda
K.mat.0<- K.mat




######## theta = pi/4 ########################
bandwidth<- c(2, .25)
intKernel.mat<- outer(evalpt, evalpt, gaussian.kernel, bw=bandwidth, theta=pi/4)
K.mat<- t(quadWgts.mat) %*% intKernel.mat %*% quadWgts.mat
as.matrix(K.mat)
niter<- as.integer(1)

##### initial value #####
Lambda.old<- matrix(1, nrow=1, ncol=length(centroid));
#Lambda.old[1, c(17, 33, 49, 65)]<- .25

#### EMS iteration ####
repeat {
    ##### EM step #####
    V.inv<- diag(1/as.vector(Lambda.old %*% t(incident.mat)))
    tmpEM<- (observed %*% V.inv %*% incident.mat) %*% diag(as.vector(Lambda.old)) %*% diag(1/8, 81)
    ##### S step #####
    Lambda<-  tmpEM %*% K.mat

    if (sum( (as.vector(as.matrix(Lambda)) - as.vector(as.matrix(Lambda.old)))^2) > tol) {
        Lambda.old<- as.vector(as.matrix(Lambda));
        niter<- as.integer(1+niter);
    }
    else {
        #lambda<- tmpEM %*% K_x.mat;
        Lambda<- as.vector(as.matrix(Lambda))
        break;
    }
}
Lambda.1<- Lambda
K.mat.1<- K.mat


########### theta = -pi/4 #############

intKernel.mat<- outer(evalpt, evalpt, gaussian.kernel, bw=bandwidth, theta=-pi/4)
K.mat<- t(quadWgts.mat) %*% intKernel.mat %*% quadWgts.mat
#as.matrix(K.mat)
##### initial value #####
niter<- as.integer(1)
Lambda.old<- matrix(1, nrow=1, ncol=length(centroid));

#### EMS iteration ####
repeat {
    ##### EM step #####
    V.inv<- diag(1/as.vector(Lambda.old %*% t(incident.mat)))
    tmpEM<- (observed %*% V.inv %*% incident.mat) %*% diag(as.vector(Lambda.old)) %*% diag(1/8, 81)
    ##### S step #####
    Lambda<-  tmpEM %*% K.mat

    if (sum( (as.vector(as.matrix(Lambda)) - as.vector(as.matrix(Lambda.old)))^2) > tol) {
        Lambda.old<- as.vector(as.matrix(Lambda));
        niter<- as.integer(1+niter);
    }
    else {
        #lambda<- tmpEM %*% K_x.mat;
        Lambda<- as.vector(as.matrix(Lambda))
        break;
    }
}
Lambda.2<- Lambda
K.mat.2<- K.mat

rm("intKernel.mat"); rm("K.mat"); rm("Lambda")
save.image("bivariate_interval_censored.RData")

### plot the estimate ####
Lambda.3<- round(Lambda.3, 4) 
Lambda.4<- Lambda.5<- rep(0, 81)
Lambda.4[c(11, 31, 51, 71)]<- Lambda.5[c(17, 33, 49, 65)]<- .25

postscript("npmle_1_07mar2009.ps", paper="letter")
plot.em.1<- levelplot(Lambda.3 ~ Re(centroid)*Im(centroid), xlab="", ylab="Time to AIDS", main="EM", colorkey=FALSE, at=seq(0, .25, by=1/256), col.regions=gray(seq(1, 0, by=-1/256)), aspect=1)
plot.em.1
dev.off()

postscript("npmle_2_07mar2009.ps", paper="letter")
plot.em.2<- levelplot(Lambda.4 ~ Re(centroid)*Im(centroid), xlab="HIV Infection Time", ylab="", main="EM", colorkey=FALSE, at=seq(0, .25, by=1/256), col.regions=gray(seq(1, 0, by=-1/256)), aspect=1)
plot.em.2
dev.off()

postscript("npmle_3_07mar2009.ps", paper="letter")
plot.em.3<- levelplot(Lambda.5 ~ Re(centroid)*Im(centroid), xlab="", ylab="", main="EM", colorkey=FALSE, at=seq(0, .25, by=1/256), col.regions=gray(seq(1, 0, by=-1/256)), aspect=1)
plot.em.3
dev.off()

#trellis.focus("panel", 1, 1); 
#ltext(x=Re(centroid), y=Im(centroid), as.character(round(Lambda.3, 4)))


postscript("ems_1_08mar2009.ps", paper="letter")
plot0<- levelplot(Lambda.0 ~ Re(centroid)*Im(centroid), xlab="", ylab="Time to AIDS", main=expression(paste(theta, "=", 0)), colorkey=FALSE, at=seq(0, .03, by=.0015), col.regions=gray(seq(1, .25, by=-.01)), aspect=1)
plot0
dev.off()
#trellis.focus("panel", 1, 1); 
#ltext(x=Re(centroid), y=Im(centroid), as.character(round(Lambda.0, 4)))

postscript("ems_2_08mar2009.ps", paper="letter")
plot1<- levelplot(Lambda.1 ~ Re(centroid)*Im(centroid), xlab="HIV Infection Time", ylab="", main=expression(paste(theta, "=", pi/4)), colorkey=FALSE, at=seq(0, .03, by=.0015), col.regions=gray(seq(1, 0.25, by=-.01)), aspect=1)
plot1
dev.off()
#plot1<- levelplot(Lambda.1 ~ Re(centroid)*Im(centroid), xlab="HIV Infection Time", ylab="Time to AIDS", main=expression(paste(theta, "=", pi/4)), colorkey=list(space="bottom", at=seq(0, .03, by=.0015)), cuts=10)
#trellis.focus("panel", 1, 1); 
#ltext(x=Re(centroid), y=Im(centroid), as.character(round(Lambda.1, 4)))

postscript("ems_3_08mar2009.ps", paper="letter")
plot2<- levelplot(Lambda.2 ~ Re(centroid)*Im(centroid), xlab="", ylab="", main=expression(paste(theta, "=", -pi/4)), colorkey=FALSE, at=seq(0, .03, by=.0015), col.regions=gray(seq(1, 0.25, by=-.01)), aspect=1)
plot2
dev.off()
#, colorkey=list(space="right", at=seq(0, .03, by=.0015), col=gray(seq(.75, 0.15, by=-.01))))
#trellis.focus("panel", 1, 1); 
#ltext(x=Re(centroid), y=Im(centroid), as.character(round(Lambda.2, 4)))



postscript("bivariate_interval_censored_30jan2009.ps", paper="letter")
print(plot0, position=c(0, 0, 8/27, 1), more=TRUE)
print(plot1, position=c(1/3, 0, 17/27, 1), more=TRUE)
print(plot2, position=c(2/3, 0, 1, 1))
dev.off()


postscript(file="bivariate_example.ps", paper="letter")
plot(c(0, 9), c(0, 9), xlim=c(0, 9), ylim=c(0, 9), type= "n", xlab="HIV Infection Time", ylab="Time to AIDS", axes=FALSE, frame=FALSE, main="Bivariate Interval Censored Data")
rect(0, 0, 9, 9); axis(1, at=0:9); axis(2, at=0:9)
rect(0, 1, 9, 2, lwd=2, density=10, angle=45);
rect(0, 3, 9, 4, lwd=2, density=10, angle=45);
rect(0, 5, 9, 6, lwd=2, density=10, angle=45);
rect(0, 7, 9, 8, lwd=2, density=10, angle=45);
rect(1, 0, 2, 9, lwd=2, density=10, angle=-45);
rect(3, 0, 4, 9, lwd=2, density=10, angle=-45);
rect(5, 0, 6, 9, lwd=2, density=10, angle=-45);
rect(7, 0, 8, 9, lwd=2, density=10, angle=-45);
dev.off()


rect(1, 1, 2, 2, border="transparent", col="grey")
rect(3, 1, 4, 2, border="transparent", col="grey")
rect(5, 1, 6, 2, border="transparent", col="grey")
rect(7, 1, 8, 2, border="transparent", col="grey")

rect(1, 3, 2, 4, border="transparent", col="grey")
rect(3, 3, 4, 4, border="transparent", col="grey")
rect(5, 3, 6, 4, border="transparent", col="grey")
rect(7, 3, 8, 4, border="transparent", col="grey")

rect(1, 5, 2, 6, border="transparent", col="grey")
rect(3, 5, 4, 6, border="transparent", col="grey")
rect(5, 5, 6, 6, border="transparent", col="grey")
rect(7, 5, 8, 6, border="transparent", col="grey")

rect(1, 7, 2, 8, border="transparent", col="grey")
rect(3, 7, 4, 8, border="transparent", col="grey")
rect(5, 7, 6, 8, border="transparent", col="grey")
rect(7, 7, 8, 8, border="transparent", col="grey")
